/**
 * @file posture.c
 * @author your name (you@domain.com)
 * @brief 车辆电机运动姿态
 * @version 0.1
 * @date 2024-09-12
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "posture.h"
#include "bsp_motor.h"
#include "bsp_SysTick.h"
#include "bsp_servo.h"
#include "bsp_sensor.h"
#include "bsp_adc.h"
#include "bsp_voice.h"
#include "reset.h"
#include "trace.h"
#include "pid_turn.h"
#include "turn.h"

extern int cnt_hdl;
extern int cnt_hdr;

/**
 * @brief 左右轮走直线, 不带时间
 * @param run_speed 速度
 */
void Run(int run_speed)
{
    Motor(1, run_speed);
    Motor(2, run_speed);
}
/**
 * @brief 左右轮走直线, 带时间
 * @param run_speed 速度
 * @param last_time 延时
 */
void Run_delay(int run_speed, uint16_t last_time)
{
    Motor(1, run_speed);
    Motor(2, run_speed);
    delay_ms(last_time);
}
/**
 * @brief 左右轮差速走,不带时间
 * @param left_speed 左轮速度
 * @param right_speed 右轮速度
 */
void run(int left_speed, int right_speed)
{
    Motor(2, left_speed);
    Motor(1, right_speed);
}
/**
 * @brief 左右轮差速走,带时间
 * @param left_speed 左轮速度
 * @param right_speed 右轮速度
 * @param last_time 延时
 */
void run_delay(int left_speed, int right_speed, uint16_t last_time)
{
    Motor(2, left_speed);
    Motor(1, right_speed);
    delay_ms(last_time);
}
/**
 * @brief 停车无限长时间
 */
void stop(void)
{
    Motor(1, 0);
    Motor(2, 0);
    Motor(1, -0);
    Motor(2, -0);
}
/**
 * @brief 停车一段时间
 * @param times 时间
 */
void Stop(uint16_t times)
{
    Motor(1, 0);
    Motor(2, 0);
    delay_ms(times);
}
/**
 * @brief 左轮维持原本速度,右轮速度减小
 *
 * @param Fir 起始速度
 * @param End 终止速度
 * @param delay_time 短延时时间 短的用3 长的用5
 */
void Right_Speed_Down(int Fir, int End, uint16_t delay_time)
{
    for (; End <= Fir; Fir--) {
        Motor(1, Fir);
        if (Fir % 5 == 0) delay_ms(delay_time);
    }
}
/**
 * @brief 右轮维持原本速度,左轮速度减小
 *
 * @param Fir 起始速度
 * @param End 终止速度
 * @param delay_time 短延时时间 短的用3 长的用5
 */
void Left_Speed_Down(int Fir, int End, uint16_t delay_time)
{
    for (; End <= Fir; Fir--) {
        Motor(2, Fir);
        if (Fir % 5 == 0) delay_ms(delay_time); // 短的用3 长的用5
    }
}
/**
 * @brief 左轮维持原本速度,右轮速度增大
 *
 * @param Fir 起始速度
 * @param End 终止速度
 * @param delay_time 短延时时间 短的用3 长的用5
 */
void Right_Speed_Up(int Fir, int End, uint16_t delay_time)
{
    for (; Fir <= End; Fir++) {
        Motor(1, Fir);
        if (Fir % 5 == 0) delay_ms(delay_time); // 短的用3 长的用5
    }
}
/**
 * @brief 右轮维持原本速度,左轮速度增大
 *
 * @param Fir 起始速度
 * @param End 终止速度
 * @param delay_time 短延时时间 短的用3 长的用5
 */
void Left_Speed_Up(int Fir, int End, uint16_t delay_time)
{
    for (; Fir <= End; Fir++) {
        Motor(2, Fir);
        if (Fir % 5 == 0) delay_ms(delay_time); // 短的用3 长的用5
    }
}
/**
 * @brief 原地加速右转,相反轮差
 *
 * @param Low 起始速度
 * @param High 终止速度
 * @param delay_time 短延时时间 用1
 */
void turnR_speed_up(int Low, int High, uint16_t delay_time)
{
    for (; Low < High; Low++) {
        Motor(2, Low);
        Motor(1, -Low);
        if (Low % 5 == 0) delay_ms(delay_time); // 用1
    }
}
/**
 * @brief 原地减速右转,相反轮差
 *
 * @param Low 起始速度
 * @param High 终止速度
 * @param delay_time 短延时时间 用1
 */
void turnR_speed_down(int High, int Low, uint16_t delay_time)
{
    for (; High > Low; High--) {
        Motor(2, High);
        Motor(1, -High);
        if (High % 5 == 0) delay_ms(delay_time); // 用1
    }
}
/**
 * @brief 原地加速左转,相反轮差
 *
 * @param Low 起始速度
 * @param High 终止速度
 * @param delay_time 短延时时间 用1
 */
void turnL_speed_up(int Low, int High, uint16_t delay_time)
{
    for (; Low < High; Low++) {
        Motor(2, -Low);
        Motor(1, Low);
        if (Low % 5 == 0) delay_ms(delay_time); // 用1
    }
}
/**
 * @brief 原地减速左转,相反轮差
 *
 * @param Low 起始速度
 * @param High 终止速度
 * @param delay_time 短延时时间 用1
 */
void turnL_speed_down(int High, int Low, uint16_t delay_time)
{
    for (; High > Low; High--) {
        Motor(2, -High);
        Motor(1, High);
        if (High % 5 == 0) delay_ms(delay_time); // 用1
    }
}
/**
 * @brief 走圆圈
 *
 * @param speed 速度
 * @param model 模式
 */
void drift_right(int speed, uint8_t model)
{
    if (Huidu_va(1) > white) {
        run(speed + 15, 0);
    } else if (Huidu_va(0) > white) {
        run(speed + 15, 10);
    } else if (Huidu_va(3) > white) {
        run(speed + 15, 20);
    } else if (Huidu_va(2) > white) {
        run(speed + 15, 30);
    } else if (Huidu_va(5) > white) {
        run(speed + 15, 30);
    } else {
        if (model == 0) {
            slow_run(speed);
        } else {
            high_run(speed);
        }
    }
}
/**
 * @brief 禁行板
 */
void Start(void)
{
    while (hwr == 0) {
        stop();
    }
    while (hwr == 1) {
        stop();
    }
}
/**
 * @brief 低速下平台
 */
void down_pt1_6(void)
{
    Front_down();
    cnt_hdl = 0;
    cnt_hdr = 0;
    while (1) {
        slow_run(40);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    while (hwr == 0) {
        slow_run(40);
    }
    Front_mid();
    Reset(60, 45);
    stop();
}
/**
 * @brief 撞景点
 * @param num 景点编号
 */
void Crash_JD(uint8_t num)
{
    if (num == 1 || num == 2) {
        Front_mid();
        // 巡线撞景点5
        while (hwl == 0) {
            slow_run(50);
        }
        // Stop(10);
        sound(10);
        run_delay(-50, -50, 150);
        stop();
    } else if (num == 3 || num == 4 || num == 5) {
        Front_mid();
        // 巡线撞景点5
        while (hwl == 0) {
            slow_run(50);
        }
        sound(10);
        Stop(10);
        cnt_hdr = 0;
        while (cnt_hdr == 0) {
            run(-40, -40);
        }
        stop();
    } else {
        stop();
    }
}
/**
 * @brief 加减速测试
 *
 */
void trace_Test(void)
{
    speed_up(50, 180);
    speed_down(180, 50);
}
/**
 * @brief 上台1到台6+动作
 *
 */
void UP_Tai1_6(void)
{
    while (hwr == 0) {
        slow_run(50);
    }
    cnt_hdr = 0;
    cnt_hdl = 0;
    while (1) {
        slow_run(55);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    while (hwl == 0) {
        run(45, 45);
    }
    Stop(50);
    run_delay(-40, -40, 100);
    Tai1_6_zhuan();
}
/**
 * @brief 上台7+动作
 *
 */
void UP_Tai7(void)
{
    while (hwr == 0) {
        slow_run(50);
    }
    Reset(100, 70);
    speed_up(70, 120);
    speed_down(120, 70);
    Front_down();
    cnt_hdr = 0;
    cnt_hdl = 0;
    while (1) {
        slow_run(60);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    while (hwl == 0) {
        run(45, 45);
    }
    Stop(50);
    run_delay(-39, -40, 130);
    Tai7_zhuan();
}
/**
 * @brief 上台8+动作
 *
 */
void UP_Tai8(void)
{
    // 上台8
    while (hwr == 0) {
        slow_run(60);
    }
    Reset(100, 70);
    // 低斜坡加速
    speed_up(70, 110);
    speed_down(110, 70);
    Front_down();
    // 检测衔接平台
    cnt_hdr = 0;
    cnt_hdl = 0;
    while (1) {
        slow_run(60);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    // 衔接平台盲走
    while (hwr == 0) {
        run(50, 50);
    }
    Reset(100, 70);
    // 高斜坡加速
    speed_up(70, 110);
    speed_down(110, 70);
    Front_down();
    // 检测高平台
    cnt_hdr = 0;
    cnt_hdl = 0;
    while (1) {
        slow_run(60);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    while (hwl == 0) {
        run(40, 40);
    }
    Stop(100);
    run_delay(-36, -40, 130);
    Tai8_zhuan();
}
/**
 * @brief 上台1+动作
 *
 */
void UP_Tai1(void)
{
    Front_down();
    while (hwr == 0) {
        slow_run(50);
    }
    cnt_hdr = 0;
    cnt_hdl = 0;
    while (1) {
        slow_run(60);
        if (cnt_hdl != 0 || cnt_hdr != 0) {
            break;
        }
    }
    run_delay(40, 40, 200);

    Tai1_6_zhuan();
}
/**
 * @brief 过桥
 */
void Bridge_Travel(void)
{
    while (hwr == 0) {
        slow_run(50);
    }
    Front_down();
    Run_delay(50, 900);
    Reset_bridge(60, 80, 1);
    Reset_bridge(60, 120, 1);
    Reset_bridge(60, 50, 2);
    while (hwr == 0) {
        slow_run(50);
    }
    // land
    Reset(50, 50);
}
/**
 * @brief 快速过梯形山 不停车
 */
void T_Moutain_Fast(void)
{
    Front_down();
    while (hwr == 0) {
        slow_run(50);
    }
    Stop(50);
    Reset(340, 50);
    Front_mid();
}
/**
 * @brief 慢速过梯形山 停车
 *
 */
void T_Moutain_Slow(void)
{
    Front_down();
    while (hwr == 0) {
        slow_run(50);
    }
    Stop(50);
    Reset(200, 50);
    Stop(50);
    Reset(130, 50);
    Front_mid();
}
/**
 * @brief 进长波浪板
 *
 */
void IN_Long_BLB(void)
{
    Front_mid();
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        slow_run(50);
    }
    Reset(380, 50);
}

/**
 * @brief 出长波浪板
 *
 */
void OUT_Long_BLB(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        slow_run(50);
    }
    Front_mid();
    Reset(420, 50);
}
/**
 * @brief 下台8
 *
 */
void Down_Tai8(void)
{
    Front_down();
    // 高斜坡巡线
    Reset(200, 50);
    // 检测衔接平台
    while (hwr == 0) {
        slow_run(50);
    }
    // 衔接平台盲走
    run_delay(40, 40, 400);
    // 检测低斜坡
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        slow_run(40);
    }
    // 拉前铲
    Front_mid();
    // 检测绿地
    while (hwr == 0) {
        slow_run(50);
    }
}
/**
 * @brief 下台7
 *
 */
void Down_Tai7(void)
{
    Reset(200, 50);
    while (hwr == 0) {
        slow_run(50);
    }
}
/**
 * @brief 停止线
 *
 */
void Stop_Line(void)
{
    while (1) {
        slow_run(60);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
}

void Pass_Short_BLB(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        slow_run(50);
    }
    Reset(280, 50);
}

void Detect_OUT_jd4(void)
{
    while (1) {
        slow_run(60);
        if (Huidu_va(0) > white || Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
}